
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_aircraft.c
  * @author     baiyang
  * @date       2022-11-28
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sim_aircraft.h"

#include <sys/unistd.h>
#include <rtthread.h>

#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <gcs_mavlink/gcs.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/geo/declination.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/
#define MAX_THERMALS 10
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_aircraft_ctor(sim_aircraft_t aircraft, sim_aircraft_ops_t ops, const char *frame_str)
{
    memset(aircraft, 0, sizeof(struct sim_aircraft));

    aircraft->ops = ops;

    aircraft->accel_body.x = 0.0f;
    aircraft->accel_body.y = 0.0f;
    aircraft->accel_body.z = -GRAVITY_MSS;

    aircraft->battery_voltage = -1;

    aircraft->gyro_noise = radians(0.1f);
    aircraft->accel_noise = 0.3f;

    aircraft->rate_hz = 500.0f;

    aircraft->use_time_sync = true;
    aircraft->last_speedup  = -1.0f;
    aircraft->config_       = "";

#if defined(__CYGWIN__) || defined(__CYGWIN64__)
    aircraft->min_sleep_time = 20000;
#else
    aircraft->min_sleep_time = 5000;
#endif

    aircraft->frame = frame_str;

    aircraft->sitl = sim_get_singleton();

    sim_aircraft_set_speedup(aircraft, 1.0f);

    aircraft->last_wall_time_us = sim_aircraft_get_wall_time_us();

    // allow for orientation settings, such as with tailsitters
    enum param_type_t ptype;
    aircraft->ahrs_orientation = (Param_int8 *)param_find_variable("AHRS_ORIENTATION", &ptype);

    // ahrs_orientation->get() returns ROTATION_NONE here, regardless of the actual value
    enum RotationEnum imu_rotation = aircraft->ahrs_orientation ? (enum RotationEnum)(*aircraft->ahrs_orientation) : ROTATION_NONE;

    aircraft->last_imu_rotation = imu_rotation;

    // sitl is null if running example program
    if (aircraft->sitl) {
        matrix3f_from_rotation(&aircraft->sitl->ahrs_rotation , imu_rotation);
        aircraft->sitl->ahrs_rotation_inv = matrix3f_transposed(&aircraft->sitl->ahrs_rotation);
    }

    // init rangefinder array to -1 to signify no data
    for (uint8_t i = 0; i < ARRAY_SIZE(aircraft->rangefinder_m); i++){
        aircraft->rangefinder_m[i] = -1.0f;
    }
}

/*
  set simulation speedup
 */
void sim_aircraft_set_speedup(sim_aircraft_t aircraft, float speedup)
{
    sim_aircraft_setup_frame_time(aircraft, aircraft->rate_hz, speedup);
}

// potentially replace this with a call to AP_HAL::Util::get_hw_rtc
uint64_t sim_aircraft_get_wall_time_us()
{
#if 0
#if defined(__CYGWIN__) || defined(__CYGWIN64__)
    static DWORD tPrev;
    static uint64_t last_ret_us;
    if (tPrev == 0) {
        tPrev = timeGetTime();
        return 0;
    }
    DWORD now = timeGetTime();
    last_ret_us += (uint64_t)((now - tPrev)*1000UL);
    tPrev = now;
    return last_ret_us;
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return uint64_t(ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL);
#else
    return time_micros64();
#endif
#endif

    return time_micros64();
}

/*
   return difference in altitude between home position and current loc
*/
float sim_aircraft_ground_height_difference(sim_aircraft_const_t aircraft)
{
#if 0
#if AP_TERRAIN_AVAILABLE
    AP_Terrain *terrain = AP::terrain();
    float h1, h2;
    if (sitl &&
        terrain != nullptr &&
        sitl->terrain_enable &&
        terrain->height_amsl(home, h1, false) &&
        terrain->height_amsl(location, h2, false)) {
        h2 += local_ground_level;
        return h2 - h1;
    }
#endif
#endif

    return aircraft->local_ground_level;
}

/*
   return current height above ground level (metres)
*/
float sim_aircraft_hagl(sim_aircraft_const_t aircraft)
{
    return (-aircraft->position.z) + aircraft->home.alt * 0.01f - aircraft->ground_level - aircraft->frame_height - sim_aircraft_ground_height_difference(aircraft);
}

/*
   update location from position
*/
void sim_aircraft_update_position(sim_aircraft_t aircraft)
{
    aircraft->location = aircraft->origin;
    location_offset(&aircraft->location , aircraft->position.x, aircraft->position.y);

    aircraft->location.alt  = (int32_t)(aircraft->home.alt - aircraft->position.z * 100.0f);

#if 0
    Vector3d pos_home = position;
    pos_home.xy() += home.get_distance_NE_double(origin);

    // logging of raw sitl data
    Vector3f accel_ef = dcm * accel_body;
// @LoggerMessage: SITL
// @Description: Simulation data
// @Field: TimeUS: Time since system startup
// @Field: VN: Velocity - North component
// @Field: VE: Velocity - East component
// @Field: VD: Velocity - Down component
// @Field: AN: Acceleration - North component
// @Field: AE: Acceleration - East component
// @Field: AD: Acceleration - Down component
// @Field: PN: Position - North component
// @Field: PE: Position - East component
// @Field: PD: Position - Down component
    AP::logger().WriteStreaming("SITL", "TimeUS,VN,VE,VD,AN,AE,AD,PN,PE,PD", "Qfffffffff",
                                           AP_HAL::micros64(),
                                           velocity_ef.x, velocity_ef.y, velocity_ef.z,
                                           accel_ef.x, accel_ef.y, accel_ef.z,
                                           pos_home.x, pos_home.y, pos_home.z);
#endif

    uint32_t now = time_millis();
    if (now - aircraft->last_one_hz_ms >= 1000) {
        // shift origin of position at 1Hz to current location
        // this prevents sperical errors building up in the GPS data
        aircraft->last_one_hz_ms = now;
        Vector2d diffNE = location_get_distance_ne(&aircraft->origin , &aircraft->location);

        aircraft->position.x -= diffNE.x;
        aircraft->position.y -= diffNE.y;

        aircraft->smoothing.position.x -= diffNE.x;
        aircraft->smoothing.position.y -= diffNE.y;

        aircraft->origin.lat = aircraft->location.lat;
        aircraft->origin.lng = aircraft->location.lng;
    }
}

/*
   update body magnetic field from position and rotation
*/
void sim_aircraft_update_mag_field_bf(sim_aircraft_t aircraft)
{
    // get the magnetic field intensity and orientation
    float intensity;
    float declination;
    float inclination;
    geo_get_mag_field_ef(aircraft->location.lat * 1e-7f, aircraft->location.lng * 1e-7f, &intensity, &declination, &inclination);

    // create a field vector and rotate to the required orientation
    Vector3f_t mag_ef = {1e3f * intensity, 0.0f, 0.0f};
    matrix3f_t R;
    matrix3f_from_euler(&R, 0.0f, -ToRad(inclination), ToRad(declination));
    matrix3f_mul_vec(&mag_ef, &R, &mag_ef);

    // calculate frame height above ground
    const float frame_height_agl = fmaxf((-aircraft->position.z) + aircraft->home.alt * 0.01f - aircraft->ground_level, 0.0f);

    if (!aircraft->sitl) {
        // running example program
        return;
    }

    // calculate scaling factor that varies from 1 at ground level to 1/8 at sitl->mag_anomaly_hgt
    // Assume magnetic anomaly strength scales with 1/R**3
    float anomaly_scaler = (aircraft->sitl->mag_anomaly_hgt / (frame_height_agl + aircraft->sitl->mag_anomaly_hgt));
    anomaly_scaler = anomaly_scaler * anomaly_scaler * anomaly_scaler;

    // add scaled anomaly to earth field
    mag_ef.x += aircraft->sitl->mag_anomaly_ned.x * anomaly_scaler;
    mag_ef.y += aircraft->sitl->mag_anomaly_ned.y * anomaly_scaler;
    mag_ef.z += aircraft->sitl->mag_anomaly_ned.z * anomaly_scaler;

    // Rotate into body frame
    matrix3f_mul_transpose_vec(&aircraft->mag_bf, &aircraft->dcm, &mag_ef);

    // add motor interference
    aircraft->mag_bf.x += aircraft->sitl->mag_mot.x * aircraft->battery_current;
    aircraft->mag_bf.y += aircraft->sitl->mag_mot.y * aircraft->battery_current;
    aircraft->mag_bf.z += aircraft->sitl->mag_mot.z * aircraft->battery_current;
}

/* advance time by deltat in seconds */
void sim_aircraft_time_advance(sim_aircraft_t aircraft)
{
    // we only advance time if it hasn't been advanced already by the
    // backend
    if (aircraft->last_time_us == aircraft->time_now_us) {
        aircraft->time_now_us += aircraft->frame_time_us;
    }
    aircraft->last_time_us = aircraft->time_now_us;
    if (aircraft->use_time_sync) {
        sim_aircraft_sync_frame_time(aircraft);
    }
}

/* setup the frame step time */
void sim_aircraft_setup_frame_time(sim_aircraft_t aircraft, float new_rate, float new_speedup)
{
    aircraft->rate_hz = new_rate;
    aircraft->target_speedup = new_speedup;
    aircraft->frame_time_us = (uint64_t)(1.0e6f/aircraft->rate_hz);

    aircraft->last_wall_time_us = sim_aircraft_get_wall_time_us();
}

/* adjust frame_time calculation */
void sim_aircraft_adjust_frame_time(sim_aircraft_t aircraft, float new_rate)
{
    aircraft->frame_time_us = (uint64_t)(1.0e6f/new_rate);
    aircraft->rate_hz = new_rate;
}

/*
   try to synchronise simulation time with wall clock time, taking
   into account desired speedup
   This tries to take account of possible granularity of
   get_wall_time_us() so it works reasonably well on windows
*/
void sim_aircraft_sync_frame_time(sim_aircraft_t aircraft)
{
    aircraft->frame_counter++;
    uint64_t now = sim_aircraft_get_wall_time_us();
    uint64_t dt_us = now - aircraft->last_wall_time_us;

    const float target_dt_us = 1.0e6/(aircraft->rate_hz*aircraft->target_speedup);

    // accumulate sleep debt if we're running too fast
    aircraft->sleep_debt_us += target_dt_us - dt_us;

    if (aircraft->sleep_debt_us < -1.0e5) {
        // don't let a large negative debt build up
        aircraft->sleep_debt_us = -1.0e5;
    }
    if (aircraft->sleep_debt_us > aircraft->min_sleep_time) {
        // sleep if we have built up a debt of min_sleep_tim
        usleep(aircraft->sleep_debt_us);
        aircraft->sleep_debt_us -= (sim_aircraft_get_wall_time_us() - now);
    }
    aircraft->last_wall_time_us = sim_aircraft_get_wall_time_us();

    uint32_t now_ms = aircraft->last_wall_time_us / 1000ULL;
    float dt_wall = (now_ms - aircraft->last_fps_report_ms) * 0.001;
    if (dt_wall > 2.0) {
#if 0
        const float achieved_rate_hz = (frame_counter - last_frame_count) / dt_wall;
        ::printf("Rate: target:%.1f achieved:%.1f speedup %.1f/%.1f\n",
                 rate_hz*target_speedup, achieved_rate_hz,
                 achieved_rate_hz/rate_hz, target_speedup);
#endif
        aircraft->last_frame_count = aircraft->frame_counter;
        aircraft->last_fps_report_ms = now_ms;
    }
}

/* add noise based on throttle level (from 0..1) */
void sim_aircraft_add_noise(sim_aircraft_t aircraft, float throttle)
{
    aircraft->gyro.x += sim_aircraft_rand_normal(0, 1) * aircraft->gyro_noise * fabsf(throttle);;
    aircraft->gyro.y += sim_aircraft_rand_normal(0, 1) * aircraft->gyro_noise * fabsf(throttle);;
    aircraft->gyro.z += sim_aircraft_rand_normal(0, 1) * aircraft->gyro_noise * fabsf(throttle);

    aircraft->accel_body.x += sim_aircraft_rand_normal(0, 1) * aircraft->accel_noise * fabsf(throttle);
    aircraft->accel_body.y += sim_aircraft_rand_normal(0, 1) * aircraft->accel_noise * fabsf(throttle);
    aircraft->accel_body.z += sim_aircraft_rand_normal(0, 1) * aircraft->accel_noise * fabsf(throttle);
}

/*
  normal distribution random numbers
  See
  http://en.literateprograms.org/index.php?title=Special:DownloadCode/Box-Muller_transform_%28C%29&oldid=7011
*/
double sim_aircraft_rand_normal(double mean, double stddev)
{
    static double n2 = 0.0;
    static int n2_cached = 0;
    if (!n2_cached) {
        double x, y, r;
        do
        {
            x = 2.0 * rand()/RAND_MAX - 1;
            y = 2.0 * rand()/RAND_MAX - 1;
            r = x*x + y*y;
        } while (math_flt_zero(r) || r > 1.0);
        const double d = sqrt(-2.0 * log(r)/r);
        const double n1 = x * d;
        n2 = y * d;
        const double result = n1 * stddev + mean;
        n2_cached = 1;
        return result;
    } else {
        n2_cached = 0;
        return n2 * stddev + mean;
    }
}

/*
   fill a sitl_fdm structure from the simulator state
*/
void sim_aircraft_fill_fdm(sim_aircraft_t aircraft, struct sitl_fdm *fdm)
{
    bool is_smoothed = false;
    if (aircraft->use_smoothing) {
        sim_aircraft_smooth_sensors(aircraft);
        is_smoothed = true;
    }
    fdm->timestamp_us = aircraft->time_now_us;
    if (fdm->home.lat == 0 && fdm->home.lng == 0) {
        // initialise home
        fdm->home = aircraft->home;
    }
    fdm->is_lock_step_scheduled = aircraft->lock_step_scheduled;
    fdm->latitude  = aircraft->location.lat * 1.0e-7;
    fdm->longitude = aircraft->location.lng * 1.0e-7;
    fdm->altitude  = aircraft->location.alt * 1.0e-2;
    fdm->heading   = degrees(atan2f(aircraft->velocity_ef.y, aircraft->velocity_ef.x));
    fdm->speedN    = aircraft->velocity_ef.x;
    fdm->speedE    = aircraft->velocity_ef.y;
    fdm->speedD    = aircraft->velocity_ef.z;
    fdm->xAccel    = aircraft->accel_body.x;
    fdm->yAccel    = aircraft->accel_body.y;
    fdm->zAccel    = aircraft->accel_body.z;
    fdm->rollRate  = degrees(aircraft->gyro.x);
    fdm->pitchRate = degrees(aircraft->gyro.y);
    fdm->yawRate   = degrees(aircraft->gyro.z);
    float r, p, y;
    matrix3f_to_euler(&aircraft->dcm, &r, &p, &y);
    fdm->rollDeg  = degrees(r);
    fdm->pitchDeg = degrees(p);
    fdm->yawDeg   = degrees(y);
    quat_from_rotation_matrix(&fdm->quaternion , &aircraft->dcm);
    fdm->airspeed = aircraft->airspeed_pitot;
    fdm->velocity_air_bf = aircraft->velocity_air_bf;
    fdm->battery_voltage = aircraft->battery_voltage;
    fdm->battery_current = aircraft->battery_current;
    fdm->motor_mask = aircraft->motor_mask | aircraft->sitl->vibe_motor_mask;
    memcpy(fdm->rpm, aircraft->rpm, sizeof(fdm->rpm));
    fdm->rcin_chan_count = aircraft->rcin_chan_count;
    fdm->range = sim_aircraft_rangefinder_range(aircraft);
    memcpy(fdm->rcin, aircraft->rcin, aircraft->rcin_chan_count * sizeof(float));
    fdm->bodyMagField = aircraft->mag_bf;

    // copy laser scanner results
    fdm->scanner.points = aircraft->scanner.points;
    fdm->scanner.ranges = aircraft->scanner.ranges;

    // copy rangefinder
    memcpy(fdm->rangefinder_m, aircraft->rangefinder_m, sizeof(fdm->rangefinder_m));

    fdm->wind_vane_apparent.direction = aircraft->wind_vane_apparent.direction;
    fdm->wind_vane_apparent.speed = aircraft->wind_vane_apparent.speed;

    fdm->wind_ef = aircraft->wind_ef;

    if (is_smoothed) {
        fdm->xAccel = aircraft->smoothing.accel_body.x;
        fdm->yAccel = aircraft->smoothing.accel_body.y;
        fdm->zAccel = aircraft->smoothing.accel_body.z;
        fdm->rollRate  = degrees(aircraft->smoothing.gyro.x);
        fdm->pitchRate = degrees(aircraft->smoothing.gyro.y);
        fdm->yawRate   = degrees(aircraft->smoothing.gyro.z);
        fdm->speedN    = aircraft->smoothing.velocity_ef.x;
        fdm->speedE    = aircraft->smoothing.velocity_ef.y;
        fdm->speedD    = aircraft->smoothing.velocity_ef.z;
        fdm->latitude  = aircraft->smoothing.location.lat * 1.0e-7;
        fdm->longitude = aircraft->smoothing.location.lng * 1.0e-7;
        fdm->altitude  = aircraft->smoothing.location.alt * 1.0e-2;
    }


    if (aircraft->ahrs_orientation != NULL) {
        enum RotationEnum imu_rotation = (enum RotationEnum)(*aircraft->ahrs_orientation);
        if (imu_rotation != aircraft->last_imu_rotation) {
            matrix3f_from_rotation(&aircraft->sitl->ahrs_rotation , imu_rotation);
            aircraft->sitl->ahrs_rotation_inv = matrix3f_transposed(&aircraft->sitl->ahrs_rotation);
            aircraft->last_imu_rotation = imu_rotation;
        }
        if (imu_rotation != ROTATION_NONE) {
            matrix3f_t m = aircraft->dcm;
            matrix3f_mul(&m, &m, &aircraft->sitl->ahrs_rotation_inv);

            matrix3f_to_euler(&m, &r, &p, &y);
            fdm->rollDeg  = degrees(r);
            fdm->pitchDeg = degrees(p);
            fdm->yawDeg   = degrees(y);
            quat_from_rotation_matrix(&fdm->quaternion, &m);
        }
    }

    // in the first call here, if a speedup option is specified, overwrite it
    if (math_flt_equal(aircraft->last_speedup, -1.0f) && !math_flt_equal(sim_aircraft_get_speedup(aircraft), 1.0f)) {
        aircraft->sitl->speedup = sim_aircraft_get_speedup(aircraft);
    }
    
    if (!math_flt_equal(aircraft->last_speedup, (float)(aircraft->sitl->speedup)) && aircraft->sitl->speedup > 0) {
        sim_aircraft_set_speedup(aircraft, aircraft->sitl->speedup);
        aircraft->last_speedup = aircraft->sitl->speedup;
    }

#if 0
    // for EKF comparison log relhome pos and velocity at loop rate
    static uint16_t last_ticks;
    uint16_t ticks = AP::scheduler().ticks();
    if (last_ticks != ticks) {
        last_ticks = ticks;
// @LoggerMessage: SIM2
// @Description: Additional simulator state
// @Field: TimeUS: Time since system startup
// @Field: PN: North position from home
// @Field: PE: East position from home
// @Field: PD: Down position from home
// @Field: VN: Velocity north
// @Field: VE: Velocity east
// @Field: VD: Velocity down
// @Field: As: Airspeed
        Vector3d pos = get_position_relhome();
        Vector3f_t vel = get_velocity_ef();
        AP::logger().WriteStreaming("SIM2", "TimeUS,PN,PE,PD,VN,VE,VD,As",
                                    "Qdddffff",
                                    time_micros64(),
                                    pos.x, pos.y, pos.z,
                                    vel.x, vel.y, vel.z,
                                    aircraft->airspeed_pitot);
    }
#endif
}

/*
  smooth sensors for kinematic consistancy when we interact with the ground
 */
void sim_aircraft_smooth_sensors(sim_aircraft_t aircraft)
{
    uint64_t now = aircraft->time_now_us;
    Vector3d delta_pos = {aircraft->position.x - aircraft->smoothing.position.x,
                          aircraft->position.y - aircraft->smoothing.position.y,
                          aircraft->position.z - aircraft->smoothing.position.z};

    if (aircraft->smoothing.last_update_us == 0 || vec3_length(&delta_pos) > 10) {
        aircraft->smoothing.position = aircraft->position;
        aircraft->smoothing.rotation_b2e = aircraft->dcm;
        aircraft->smoothing.accel_body = aircraft->accel_body;
        aircraft->smoothing.velocity_ef = aircraft->velocity_ef;
        aircraft->smoothing.gyro = aircraft->gyro;
        aircraft->smoothing.last_update_us = now;
        aircraft->smoothing.location = aircraft->location;
        printf("Smoothing reset at %.3f\n", now * 1.0e-6f);
        return;
    }
    const float delta_time = (now - aircraft->smoothing.last_update_us) * 1.0e-6f;
    if (delta_time < 0 || delta_time > 0.1) {
        return;
    }

    // calculate required accel to get us to desired position and velocity in the time_constant
    const float time_constant = 0.1f;
    Vector3f_t dvel = {(aircraft->velocity_ef.x - aircraft->smoothing.velocity_ef.x) + (delta_pos.x / time_constant),
                       (aircraft->velocity_ef.y - aircraft->smoothing.velocity_ef.y) + (delta_pos.y / time_constant),
                       (aircraft->velocity_ef.z - aircraft->smoothing.velocity_ef.z) + (delta_pos.z / time_constant)};

    Vector3f_t accel_e;
    matrix3f_mul_vec(&accel_e, &aircraft->dcm, &aircraft->accel_body);

    accel_e.x = dvel.x / time_constant + accel_e.x;
    accel_e.y = dvel.y / time_constant + accel_e.y;
    accel_e.z = dvel.z / time_constant + accel_e.z + GRAVITY_MSS;

    const float accel_limit = 14 * GRAVITY_MSS;
    accel_e.x = math_constrain_float(accel_e.x, -accel_limit, accel_limit);
    accel_e.y = math_constrain_float(accel_e.y, -accel_limit, accel_limit);
    accel_e.z = math_constrain_float(accel_e.z, -accel_limit, accel_limit);

    const Vector3f_t accel_ef = {accel_e.x, accel_e.y, accel_e.z-GRAVITY_MSS};
    matrix3f_mul_transpose_vec(&aircraft->smoothing.accel_body, &aircraft->smoothing.rotation_b2e, &accel_ef);

    // calculate rotational rate to get us to desired attitude in time constant
    Quat_t desired_q, current_q, error_q;
    quat_from_rotation_matrix(&desired_q, &aircraft->dcm);
    quat_norm(&desired_q);

    quat_from_rotation_matrix(&current_q, &aircraft->smoothing.rotation_b2e);
    quat_norm(&current_q);

    quat_div(&error_q, &desired_q, &current_q);
    quat_norm(&error_q);

    Vector3f_t angle_differential;
    quat_to_axis_angle(&error_q , &angle_differential);

    aircraft->smoothing.gyro.x = aircraft->gyro.x + angle_differential.x / time_constant;
    aircraft->smoothing.gyro.y = aircraft->gyro.y + angle_differential.y / time_constant;
    aircraft->smoothing.gyro.z = aircraft->gyro.z + angle_differential.z / time_constant;

#if 0
    float R, P, Y;
    matrix3f_to_euler(&aircraft->smoothing.rotation_b2e, &R, &P, &Y);
    float R2, P2, Y2;
    matrix3f_to_euler(&aircraft->dcm, &R2, &P2, &Y2);

// @LoggerMessage: SMOO
// @Description: Smoothed sensor data fed to EKF to avoid inconsistencies
// @Field: TimeUS: Time since system startup
// @Field: AEx: Angular Velocity (around x-axis)
// @Field: AEy: Angular Velocity (around y-axis)
// @Field: AEz: Angular Velocity (around z-axis)
// @Field: DPx: Velocity (along x-axis)
// @Field: DPy: Velocity (along y-axis)
// @Field: DPz: Velocity (along z-axis)
// @Field: R: Roll
// @Field: P: Pitch
// @Field: Y: Yaw
// @Field: R2: DCM Roll
// @Field: P2: DCM Pitch
// @Field: Y2: DCM Yaw
    AP::logger().WriteStreaming("SMOO", "TimeUS,AEx,AEy,AEz,DPx,DPy,DPz,R,P,Y,R2,P2,Y2",
                                           "Qffffffffffff",
                                           AP_HAL::micros64(),
                                           degrees(angle_differential.x),
                                           degrees(angle_differential.y),
                                           degrees(angle_differential.z),
                                           delta_pos.x, delta_pos.y, delta_pos.z,
                                           degrees(R), degrees(P), degrees(Y),
                                           degrees(R2), degrees(P2), degrees(Y2));
#endif


    // integrate to get new attitude
    const Vector3f_t delta_angle = {aircraft->smoothing.gyro.x * delta_time,
                                    aircraft->smoothing.gyro.y * delta_time,
                                    aircraft->smoothing.gyro.z * delta_time};

    matrix3f_rotate(&aircraft->smoothing.rotation_b2e, &delta_angle);
    matrix3f_normalize(&aircraft->smoothing.rotation_b2e);

    // integrate to get new position
    aircraft->smoothing.velocity_ef.x += accel_e.x * delta_time;
    aircraft->smoothing.velocity_ef.y += accel_e.y * delta_time;
    aircraft->smoothing.velocity_ef.z += accel_e.z * delta_time;

    aircraft->smoothing.position.x += aircraft->smoothing.velocity_ef.x * delta_time;
    aircraft->smoothing.position.y += aircraft->smoothing.velocity_ef.y * delta_time;
    aircraft->smoothing.position.z += aircraft->smoothing.velocity_ef.z * delta_time;

    aircraft->smoothing.location = aircraft->origin;
    location_offset(&aircraft->smoothing.location , aircraft->smoothing.position.x, aircraft->smoothing.position.y);
    aircraft->smoothing.location.alt  = (int32_t)(aircraft->home.alt - aircraft->smoothing.position.z * 100.0f);

    aircraft->smoothing.last_update_us = now;
}

float sim_aircraft_rangefinder_range(sim_aircraft_const_t aircraft)
{
    float roll = aircraft->sitl->state.rollDeg;
    float pitch = aircraft->sitl->state.pitchDeg;

    if (roll > 0) {
        roll -= sim_aircraft_rangefinder_beam_width(aircraft);
        if (roll < 0) {
            roll = 0;
        }
    } else {
        roll += sim_aircraft_rangefinder_beam_width(aircraft);
        if (roll > 0) {
            roll = 0;
        }
    }
    if (pitch > 0) {
        pitch -= sim_aircraft_rangefinder_beam_width(aircraft);
        if (pitch < 0) {
            pitch = 0;
        }
    } else {
        pitch += sim_aircraft_rangefinder_beam_width(aircraft);
        if (pitch > 0) {
            pitch = 0;
        }
    }

    if (fabs(roll) >= 90.0 || fabs(pitch) >= 90.0) {
        // not going to hit the ground....
        return INFINITY;
    }

    float altitude = sim_aircraft_perpendicular_distance_to_rangefinder_surface(aircraft);

    // sensor position offset in body frame
    const Vector3f_t relPosSensorBF = aircraft->sitl->rngfnd_pos_offset;

    // n.b. the following code is assuming rotation-pitch-270:
    // adjust altitude for position of the sensor on the vehicle if position offset is non-zero
    if (!vec3_is_zero(&relPosSensorBF)) {
        // get a rotation matrix following DCM conventions (body to earth)
        matrix3f_t rotmat;
        quat_rotation_matrix(&aircraft->sitl->state.quaternion, &rotmat);
        // rotate the offset into earth frame
        Vector3f_t relPosSensorEF;
        matrix3f_mul_vec(&relPosSensorEF, &rotmat, &relPosSensorBF);
        // correct the altitude at the sensor
        altitude -= relPosSensorEF.z;
    }

    // adjust for apparent altitude with roll
    altitude /= cosf(radians(roll)) * cosf(radians(pitch));

    // Add some noise on reading
    altitude += aircraft->sitl->sonar_noise * math_rand_float();

    return altitude;
}

void sim_aircraft_update_model(sim_aircraft_t aircraft, const struct sitl_input *input)
{
    if (!aircraft->home_is_set) {
        if (aircraft->sitl == NULL) {
            return;
        }
        Location loc = {0};
        loc.lat = aircraft->sitl->opos.lat * 1.0e7;
        loc.lng = aircraft->sitl->opos.lng * 1.0e7;
        loc.alt = aircraft->sitl->opos.alt * 1.0e2;
        sim_aircraft_set_start_location(aircraft, &loc, aircraft->sitl->opos.hdg);
    }
    aircraft->local_ground_level = 0.0f;

    sim_aircraft_update(aircraft, input);
}

/*
  update the simulation attitude and relative position
 */
void sim_aircraft_update_dynamics(sim_aircraft_t aircraft, const Vector3f_t *rot_accel)
{
    const float delta_time = aircraft->frame_time_us * 1.0e-6f;

    // update rotational rates in body frame
    aircraft->gyro.x += rot_accel->x * delta_time;
    aircraft->gyro.y += rot_accel->y * delta_time;
    aircraft->gyro.z += rot_accel->z * delta_time;

    aircraft->gyro.x = math_constrain_float(aircraft->gyro.x, -radians(2000.0f), radians(2000.0f));
    aircraft->gyro.y = math_constrain_float(aircraft->gyro.y, -radians(2000.0f), radians(2000.0f));
    aircraft->gyro.z = math_constrain_float(aircraft->gyro.z, -radians(2000.0f), radians(2000.0f));

    // update attitude
    Vector3f_t delta_angle = {aircraft->gyro.x * delta_time, aircraft->gyro.y * delta_time, aircraft->gyro.z * delta_time};
    matrix3f_rotate(&aircraft->dcm, &delta_angle);
    matrix3f_normalize(&aircraft->dcm);

    Vector3f_t accel_earth;
    matrix3f_mul_vec(&accel_earth, &aircraft->dcm, &aircraft->accel_body);

    accel_earth.z += GRAVITY_MSS;

    // if we're on the ground, then our vertical acceleration is limited
    // to zero. This effectively adds the force of the ground on the aircraft
    if (sim_aircraft_on_ground(aircraft) && accel_earth.z > 0) {
        accel_earth.z = 0;
    }

    // work out acceleration as seen by the accelerometers. It sees the kinematic
    // acceleration (ie. real movement), plus gravity
    Vector3f_t accel_ef = {accel_earth.x, accel_earth.y, accel_earth.z - GRAVITY_MSS};
    matrix3f_mul_transpose_vec(&aircraft->accel_body, &aircraft->dcm, &accel_ef);

    // new velocity vector
    aircraft->velocity_ef.x += accel_earth.x * delta_time;
    aircraft->velocity_ef.y += accel_earth.y * delta_time;
    aircraft->velocity_ef.z += accel_earth.z * delta_time;

    const bool was_on_ground = sim_aircraft_on_ground(aircraft);

    // new position vector
    aircraft->position.x += aircraft->velocity_ef.x * delta_time;
    aircraft->position.y += aircraft->velocity_ef.y * delta_time;
    aircraft->position.z += aircraft->velocity_ef.z * delta_time;

    // velocity relative to air mass, in earth frame
    aircraft->velocity_air_ef.x = aircraft->velocity_ef.x + aircraft->wind_ef.x;
    aircraft->velocity_air_ef.y = aircraft->velocity_ef.y + aircraft->wind_ef.y;
    aircraft->velocity_air_ef.z = aircraft->velocity_ef.z + aircraft->wind_ef.z;

    // velocity relative to airmass in body frame
    matrix3f_mul_transpose_vec(&aircraft->velocity_air_bf, &aircraft->dcm, &aircraft->velocity_air_ef);

    // airspeed
    aircraft->airspeed = vec3_length(&aircraft->velocity_air_ef);

    // airspeed as seen by a fwd pitot tube (limited to 120m/s)
    aircraft->airspeed_pitot = math_constrain_float(aircraft->velocity_air_bf.x, 0.0f, 120.0f);

    // constrain height to the ground
    if (sim_aircraft_on_ground(aircraft)) {
        if (!was_on_ground && time_millis() - aircraft->last_ground_contact_ms > 1000) {
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM Hit ground at %f m/s", aircraft->velocity_ef.z);
            aircraft->last_ground_contact_ms = time_millis();
        }
        aircraft->position.z = -(aircraft->ground_level + aircraft->frame_height - aircraft->home.alt * 0.01f + sim_aircraft_ground_height_difference(aircraft));

        // get speed of ground movement (for ship takeoff/landing)
        float yaw_rate = 0;
#if AP_SIM_SHIP_ENABLED
        const Vector2f_t ship_movement = sim_ship_get_ground_speed_adjustment(&aircraft->sitl->shipsim, &aircraft->location, &yaw_rate);
        const Vector3f_t gnd_movement = {ship_movement.x, ship_movement.y, 0};
#else
        const Vector3f_t gnd_movement = VECTOR3F_DEFAULT_VALUE;
#endif
        switch (aircraft->ground_behavior) {
        case GROUND_BEHAVIOR_NONE:
            break;
        case GROUND_BEHAVIOR_NO_MOVEMENT: {
            // zero roll/pitch, but keep yaw
            float r, p, y;
            matrix3f_to_euler(&aircraft->dcm, &r, &p, &y);
            y = y + yaw_rate * delta_time;
            matrix3f_from_euler(&aircraft->dcm, 0.0f, 0.0f, y);
            // X, Y movement tracks ground movement
            aircraft->velocity_ef.x = gnd_movement.x;
            aircraft->velocity_ef.y = gnd_movement.y;
            if (aircraft->velocity_ef.z > 0.0f) {
                aircraft->velocity_ef.z = 0.0f;
            }
            vec3_zero(&aircraft->gyro);
            aircraft->use_smoothing = true;
            break;
        }
        case GROUND_BEHAVIOR_FWD_ONLY: {
            // zero roll/pitch, but keep yaw
            float r, p, y;
            matrix3f_to_euler(&aircraft->dcm, &r, &p, &y);
            if (vec3_length(&aircraft->velocity_ef) < 5) {
                // at high speeds don't constrain pitch, otherwise we
                // can get stuck in takeoff
                p = 0;
            } else {
                p = MAX(p, 0);
            }
            y = y + yaw_rate * delta_time;
            matrix3f_from_euler(&aircraft->dcm, 0.0f, p, y);
            // only fwd movement
            Vector3f_t v_bf;
            matrix3f_mul_transpose_vec(&v_bf, &aircraft->dcm, &aircraft->velocity_ef);
            v_bf.y = 0.0f;
            if (v_bf.x < 0.0f) {
                v_bf.x = 0.0f;
            }

            Vector3f_t gnd_movement_bf;
            matrix3f_mul_transpose_vec(&gnd_movement_bf, &aircraft->dcm, &gnd_movement);

            // lateral speed equals ground movement
            v_bf.y = gnd_movement_bf.y;

            if (!vec3_is_zero(&gnd_movement_bf)) {
                // fwd speed slowly approaches ground movement to simulate wheel friction
                const float tconst = 20; // seconds
                const float alpha = delta_time/(delta_time+tconst/M_2PI);
                v_bf.x += (gnd_movement.x - v_bf.x) * alpha;
            }

            matrix3f_mul_vec(&aircraft->velocity_ef, &aircraft->dcm, &v_bf);
            if (aircraft->velocity_ef.z > 0.0f) {
                aircraft->velocity_ef.z = 0.0f;
            }
            vec3_zero(&aircraft->gyro);
            aircraft->gyro.z = yaw_rate;
            aircraft->use_smoothing = true;
            break;
        }
        case GROUND_BEHAVIOR_TAILSITTER: {
            // rotate normal refernce frame to get yaw angle, then rotate back
            matrix3f_t rot;
            matrix3f_from_rotation(&rot, ROTATION_PITCH_270);
            float r, p, y;
            matrix3f_t dcm_rot;
            matrix3f_mul(&dcm_rot, &aircraft->dcm, &rot);
            matrix3f_to_euler(&dcm_rot, &r, &p, &y);
            y = y + yaw_rate * delta_time;
            matrix3f_from_euler(&aircraft->dcm, 0.0, 0.0, y);
            matrix3f_from_rotation(&rot, ROTATION_PITCH_90);
            matrix3f_mul(&aircraft->dcm, &aircraft->dcm, &rot);
            // X, Y movement tracks ground movement
            aircraft->velocity_ef.x = gnd_movement.x;
            aircraft->velocity_ef.y = gnd_movement.y;
            if (aircraft->velocity_ef.z > 0.0f) {
                aircraft->velocity_ef.z = 0.0f;
            }
            vec3_zero(&aircraft->gyro);
            aircraft->gyro.x = yaw_rate;
            aircraft->use_smoothing = true;
            break;
        }
        }
    }

    // allow for changes in physics step
    sim_aircraft_adjust_frame_time(aircraft, math_constrain_float(aircraft->sitl->loop_rate_hz, aircraft->rate_hz-1, aircraft->rate_hz+1));
}

/*
  update wind vector
*/
void sim_aircraft_update_wind(sim_aircraft_t aircraft, const struct sitl_input *input)
{
    // wind vector in earth frame
    aircraft->wind_ef.x = cosf(radians(input->wind.direction))*cosf(radians(input->wind.dir_z)) * input->wind.speed;
    aircraft->wind_ef.y = sinf(radians(input->wind.direction))*cosf(radians(input->wind.dir_z)) * input->wind.speed;
    aircraft->wind_ef.z = sinf(radians(input->wind.dir_z)) * input->wind.speed;

    const Vector3f_t des_ned = location_get_distance_ned(&aircraft->home, &aircraft->origin);

    Vector3d currentPos = {aircraft->position.x + des_ned.x,
                           aircraft->position.y + des_ned.y,
                           aircraft->position.z + des_ned.z};

    aircraft->wind_ef.z += sim_aircraft_get_local_updraft(aircraft, &currentPos);

    const float wind_turb = input->wind.turbulence * 10.0f;  // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98
    const float iir_coef = 0.98f;  // filtering high frequencies from turbulence

    if (wind_turb > 0 && !sim_aircraft_on_ground(aircraft)) {

        aircraft->turbulence_azimuth = aircraft->turbulence_azimuth + (2 * rand());

        aircraft->turbulence_horizontal_speed =
                (float)(aircraft->turbulence_horizontal_speed * iir_coef+wind_turb * sim_aircraft_rand_normal(0, 1) * (1 - iir_coef));

        aircraft->turbulence_vertical_speed = (float)((aircraft->turbulence_vertical_speed * iir_coef) + (wind_turb * sim_aircraft_rand_normal(0, 1) * (1 - iir_coef)));

        aircraft->wind_ef.x += cosf(radians(aircraft->turbulence_azimuth)) * aircraft->turbulence_horizontal_speed;
        aircraft->wind_ef.y += sinf(radians(aircraft->turbulence_azimuth)) * aircraft->turbulence_horizontal_speed;
        aircraft->wind_ef.z += aircraft->turbulence_vertical_speed;
    }
}

float sim_aircraft_get_local_updraft(sim_aircraft_t aircraft, const Vector3d *currentPos)
{
    int scenario = aircraft->sitl->thermal_scenario;

    float thermals_w[MAX_THERMALS];
    float thermals_r[MAX_THERMALS];
    float thermals_x[MAX_THERMALS];
    float thermals_y[MAX_THERMALS];

    int n_thermals = 0;

    switch (scenario) {
        case 0:
            return 0;
        case 1:
            n_thermals = 1;
            thermals_w[0] =  2.0;
            thermals_r[0] =  80.0;
            thermals_x[0] = -180.0;
            thermals_y[0] = -260.0;
            break;
        case 2:
            n_thermals = 1;
            thermals_w[0] =  4.0;
            thermals_r[0] =  30.0;
            thermals_x[0] = -180.0;
            thermals_y[0] = -260.0;
            break;
        case 3:
            n_thermals = 1;
            thermals_w[0] =  2.0;
            thermals_r[0] =  30.0;
            thermals_x[0] = -180.0;
            thermals_y[0] = -260.0;
            break;
        default:
            brd_config_error("Bad thermal scenario");
    }

    // Wind drift at this altitude
    float driftX = aircraft->sitl->wind_speed * (currentPos->z+100) * cosf(aircraft->sitl->wind_direction * DEG_TO_RAD);
    float driftY = aircraft->sitl->wind_speed * (currentPos->z+100) * sinf(aircraft->sitl->wind_direction * DEG_TO_RAD);

    int iThermal;
    float w = 0.0f;
    float r2;
    for (iThermal=0;iThermal<n_thermals;iThermal++) {
        Vector3d thermalPos = { thermals_x[iThermal] + driftX/thermals_w[iThermal],
                                thermals_y[iThermal] + driftY/thermals_w[iThermal],
                                0};
        Vector3d relVec = {currentPos->x - thermalPos.x,
                           currentPos->y - thermalPos.y,
                           currentPos->z - thermalPos.z};

        r2 = relVec.x*relVec.x + relVec.y*relVec.y;
        w += thermals_w[iThermal]*exp(-r2/(thermals_r[iThermal]*thermals_r[iThermal]));
    }

    return w;
}

/*
  return a filtered servo input as a value from -1 to 1
  servo is assumed to be 1000 to 2000, trim at 1500
 */
float sim_aircraft_filtered_idx(sim_aircraft_t aircraft, float v, uint8_t idx)
{
    if (aircraft->sitl->servo_speed <= 0) {
        return v;
    }
    const float cutoff = 1.0f / (2 * M_PI * aircraft->sitl->servo_speed);
    lpf_set_cutoff1(&aircraft->servo_filter[idx], cutoff);

    if (idx >= ARRAY_SIZE(aircraft->servo_filter)) {
        console_panic("Attempt to filter invalid servo at offset %u", (unsigned)idx);
    }

    return lpf_apply2(&aircraft->servo_filter[idx], v, aircraft->frame_time_us * 1.0e-6f);
}

/*
  return a filtered servo input as a value from -1 to 1
  servo is assumed to be 1000 to 2000, trim at 1500
 */
float sim_aircraft_filtered_servo_angle(sim_aircraft_t aircraft, const struct sitl_input *input, uint8_t idx)
{
    const float v = (input->servos[idx] - 1500)/500.0f;
    return sim_aircraft_filtered_idx(aircraft, v, idx);
}

/*
  return a filtered servo input as a value from 0 to 1
  servo is assumed to be 1000 to 2000
 */
float sim_aircraft_filtered_servo_range(sim_aircraft_t aircraft, const struct sitl_input *input, uint8_t idx)
{
    const float v = (input->servos[idx] - 1000)/1000.0f;
    return sim_aircraft_filtered_idx(aircraft, v, idx);
}

// extrapolate sensors by a given delta time in seconds
void sim_aircraft_extrapolate_sensors(sim_aircraft_t aircraft, float delta_time)
{
    Vector3f_t accel_earth;
    matrix3f_mul_vec(&accel_earth, &aircraft->dcm, &aircraft->accel_body);

    accel_earth.z += GRAVITY_MSS;

    Vector3f_t delta_angle = {aircraft->gyro.x * delta_time,
                              aircraft->gyro.y * delta_time,
                              aircraft->gyro.z * delta_time};

    matrix3f_rotate(&aircraft->dcm, &delta_angle);
    matrix3f_normalize(&aircraft->dcm);

    // work out acceleration as seen by the accelerometers. It sees the kinematic
    // acceleration (ie. real movement), plus gravity
    Vector3f_t accel_ef = {accel_earth.x, accel_earth.y, accel_earth.z-GRAVITY_MSS};
    matrix3f_mul_transpose_vec(&aircraft->accel_body, &aircraft->dcm, &accel_ef);

    // new velocity and position vectors
    aircraft->velocity_ef.x += accel_earth.x * delta_time;
    aircraft->velocity_ef.y += accel_earth.y * delta_time;
    aircraft->velocity_ef.z += accel_earth.z * delta_time;

    aircraft->position.x += aircraft->velocity_ef.x * delta_time;
    aircraft->position.y += aircraft->velocity_ef.y * delta_time;
    aircraft->position.z += aircraft->velocity_ef.z * delta_time;

    aircraft->velocity_air_ef.x = aircraft->velocity_ef.x + aircraft->wind_ef.x;
    aircraft->velocity_air_ef.y = aircraft->velocity_ef.y + aircraft->wind_ef.y;
    aircraft->velocity_air_ef.z = aircraft->velocity_ef.z + aircraft->wind_ef.z;

    matrix3f_mul_transpose_vec(&aircraft->velocity_air_bf, &aircraft->dcm, &aircraft->velocity_air_ef);
}

void sim_aircraft_update_external_payload(sim_aircraft_t aircraft, const struct sitl_input *input)
{
    aircraft->external_payload_mass = 0;

#if AP_SIM_SHIP_ENABLED
    sim_ship_update(&aircraft->sitl->shipsim);
#endif
}

void sim_aircraft_add_shove_forces(sim_aircraft_t aircraft, Vector3f_t *rot_accel, Vector3f_t *body_accel)
{
    (void)rot_accel;

    const uint32_t now = time_millis();
    if (aircraft->sitl == NULL) {
        return;
    }
    if (aircraft->sitl->shove.t == 0) {
        return;
    }
    if (aircraft->sitl->shove.start_ms == 0) {
        aircraft->sitl->shove.start_ms = now;
    }
    if (now - aircraft->sitl->shove.start_ms < (uint32_t)(aircraft->sitl->shove.t)) {
        // FIXME: can we get a vector operation here instead?
        body_accel->x += aircraft->sitl->shove.x;
        body_accel->y += aircraft->sitl->shove.y;
        body_accel->z += aircraft->sitl->shove.z;
    } else {
        aircraft->sitl->shove.start_ms = 0;
        aircraft->sitl->shove.t = 0;
    }
}

void sim_aircraft_add_twist_forces(sim_aircraft_t aircraft, Vector3f_t *rot_accel)
{
    if (aircraft->sitl == NULL) {
        return;
    }
    if (aircraft->sitl->gnd_behav != -1) {
        aircraft->ground_behavior = (enum GroundBehaviour)aircraft->sitl->gnd_behav;
    }
    const uint32_t now = time_millis();
    if (aircraft->sitl == NULL) {
        return;
    }
    if (aircraft->sitl->twist.t == 0) {
        return;
    }
    if (aircraft->sitl->twist.start_ms == 0) {
        aircraft->sitl->twist.start_ms = now;
    }
    if (now - aircraft->sitl->twist.start_ms < (uint32_t)(aircraft->sitl->twist.t)) {
        // FIXME: can we get a vector operation here instead?
        rot_accel->x += aircraft->sitl->twist.x;
        rot_accel->y += aircraft->sitl->twist.y;
        rot_accel->z += aircraft->sitl->twist.z;
    } else {
        aircraft->sitl->twist.start_ms = 0;
        aircraft->sitl->twist.t = 0;
    }
}

/*
  get position relative to home
 */
Vector3d sim_aircraft_get_position_relhome(sim_aircraft_const_t aircraft)
{
    Vector3d pos = aircraft->position;
    const Vector2f_t dis_ne = location_get_distance_ne(&aircraft->home, &aircraft->origin);

    pos.x += dis_ne.x;
    pos.y += dis_ne.y;

    return pos;
}

void sim_aircraft_state_publish(sim_aircraft_const_t aircraft)
{
    uitc_vehicle_hil_state hil_state;

    hil_state.time_usec = time_micros64();
    hil_state.alt = aircraft->location.alt * 10;
    hil_state.lat = aircraft->location.lat;
    hil_state.lon = aircraft->location.lng;

    float r, p, y;
    matrix3f_to_euler(&aircraft->dcm, &r, &p, &y);
    hil_state.roll = r;
    hil_state.pitch = p;
    hil_state.yaw = y;

    hil_state.heading = atan2f(aircraft->velocity_ef.y, aircraft->velocity_ef.x);

    hil_state.rollspeed = aircraft->gyro.x;
    hil_state.pitchspeed = aircraft->gyro.y;
    hil_state.yawspeed = aircraft->gyro.z;

    hil_state.x = (int32_t)(aircraft->position.x * 100);
    hil_state.y = (int32_t)(aircraft->position.y * 100);
    hil_state.z = aircraft->location.alt - aircraft->home.alt;

    hil_state.vx = (int16_t)(aircraft->velocity_ef.x * 100);
    hil_state.vy = (int16_t)(aircraft->velocity_ef.y * 100);
    hil_state.vz = (int16_t)(aircraft->velocity_ef.z * 100);

    hil_state.xacc = (int16_t)(aircraft->accel_body.x * 1000 / GRAVITY_MSS);
    hil_state.yacc = (int16_t)(aircraft->accel_body.y * 1000 / GRAVITY_MSS);
    hil_state.zacc = (int16_t)((aircraft->accel_body.z + GRAVITY_MSS) * 1000 / GRAVITY_MSS);
    itc_publish(ITC_ID(vehicle_hil_state), &hil_state);
}


///
void sim_aircraft_SetStartLocation(sim_aircraft_t aircraft, const Location *start_loc, const float start_yaw)
{
    aircraft->home = *start_loc;
    aircraft->origin = aircraft->home;

    aircraft->position.x = 0.0f;
    aircraft->position.y = 0.0f;

    aircraft->home_yaw = start_yaw;
    aircraft->home_is_set = true;

    printf("Home: %f %f alt=%fm hdg=%f\n",
             aircraft->home.lat*1e-7,
             aircraft->home.lng*1e-7,
             aircraft->home.alt*0.01,
             aircraft->home_yaw);

    aircraft->location = aircraft->home;
    aircraft->ground_level = aircraft->home.alt * 0.01f;

#if 0
    // useful test for home position being very different from origin
    home.offset(-3000*1000, 1800*1000);
#endif

    matrix3f_from_euler(&aircraft->dcm, 0.0f, 0.0f, radians(aircraft->home_yaw));
}

// returns perpendicular height to surface downward-facing rangefinder
// is bouncing off:
float sim_aircraft_PerpendicularDistanceToRangefinderSurface(sim_aircraft_const_t aircraft)
{
    switch ((enum RotationEnum)aircraft->sitl->sonar_rot) {
    case ROTATION_PITCH_270:
        return aircraft->sitl->height_agl;
    case ROTATION_NONE:
    case ROTATION_YAW_45:
    case ROTATION_YAW_90:
    case ROTATION_YAW_135:
    case ROTATION_YAW_180:
    case ROTATION_YAW_225:
    case ROTATION_YAW_270:
    case ROTATION_YAW_315:
        return sim_measure_distance_at_angle_bf(&aircraft->location, aircraft->sitl->sonar_rot*45);
    default:
        brd_config_error("Bad simulated sonar rotation");
        break;
    }

    return 0.0f;
}

/*
   return true if we are on the ground
*/
bool sim_aircraft_OnGround(sim_aircraft_const_t aircraft)
{
    return sim_aircraft_hagl(aircraft) <= 0.001f;  // prevent bouncing around ground
}
/*------------------------------------test------------------------------------*/


